/**
 * @file binarize.cpp
 * @brief 二值化和边线提取模块实现
 * 
 * 实现图像二值化和赛道边线提取功能。
 */
#include "binarize.h"

#include <cstdint>

void binarizeImage(
    const unsigned char gray[][SCALED_WIDTH],
    unsigned char binary[][SCALED_WIDTH],
    int height, int width,
    unsigned char threshold
) 
{
   
    for (int i = 0; i < height; i++) {
        for (int j = 0; j < width; j++) {
            binary[i][j] = (gray[i][j] > threshold) ? 255 : 0;
        }
    }
}


// 8位POP计数表
static const unsigned char pop8[256] = {
    0,1,1,2,1,2,2,3,1,2,2,3,2,3,3,4,1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,
    1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
    1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
    2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
    1,2,2,3,2,3,3,4,2,3,3,4,3,4,4,5,2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,
    2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
    2,3,3,4,3,4,4,5,3,4,4,5,4,5,5,6,3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,
    3,4,4,5,4,5,5,6,4,5,5,6,5,6,6,7,4,5,5,6,5,6,6,7,5,6,6,7,6,7,7,8
};


// 合并实现：同时追踪左右边界，基于“接缝/区域”判断并包含顶端漂移防护
/**
 * @brief 同时追踪左右边线的包装函数（合并实现）
 * @note 内部实现基于“接缝/区域”判定，带有漂移限制和小范围投票搜索，便于减小顶端区域漂移。
 */
void trackEdges(
    const unsigned char bin[][SCALED_WIDTH],
    int height, int width,
    int leftCol[], int rightCol[]
)
{
    if (height <= 0 || width <= 0) return;

    for (int i = 0; i < height; ++i) {
        if (leftCol) leftCol[i] = -1;
        if (rightCol) rightCol[i] = -1;
    }

    // ========== 底行种子初始化 ==========
    int y = height - 1;
    int cx = width / 2;
    int leftX = cx, rightX = cx;

    while (leftX > 0 && bin[y][leftX - 1] == 255) --leftX;
    while (rightX < width - 1 && bin[y][rightX + 1] == 255) ++rightX;

    if (rightX - leftX + 1 < (width >> 1)) {
        return; // 白区不足 50%，丢弃整帧
    }

    leftCol[y]  = (leftX > 0) ? leftX - 1 : 0;
    rightCol[y] = (rightX < width - 1) ? rightX + 1 : width - 1;

    const int maxRadius = TRACK_MAX_RADIUS;
    const int seamHalf = TRACK_SEAM_WIDTH; // 通常 2~4，必须 <= 7

    // 位图缓冲区：支持 width <= 320 → 40 字节足够
    unsigned char bitRow[40];

    // ========== 从底向上逐行追踪 ==========
    for (int row = y - 1; row >= 0; --row) {
        int prevL = leftCol[row + 1];
        int prevR = rightCol[row + 1];

        // ---------- 打包当前行为位图（低位 = 左列） ----------
        int nBytes = (width + 7) / 8;
        for (int i = 0; i < nBytes; ++i) {
            unsigned char byte = 0;
            for (int j = 0; j < 8; ++j) {
                int col = i * 8 + j;
                if (col >= width) break;
                if (bin[row][col] == 255) { // 白背景 → 1
                    byte |= (1U << j);      // col 对应 bit j（低位=左）
                }
            }
            bitRow[i] = byte;
        }

        // ---------- 辅助函数：计算 [start, start+len) 中白像素数量 ----------
        auto popWhite = [&](int start, int len) -> int {
            if (len <= 0 || start >= width) return 0;
             int remaining = width - start;
            if (len > remaining) len = remaining;
            if (len <= 0) return 0;

            int byteIdx = start >> 3; // start / 8
            uint16_t word;
            if (byteIdx + 1 < nBytes) {
                word = static_cast<uint16_t>(bitRow[byteIdx]) |
                       (static_cast<uint16_t>(bitRow[byteIdx + 1]) << 8);
            } else {
                word = bitRow[byteIdx];
            }

            int offset = start & 7; // start % 8
            uint16_t aligned = word >> offset;
            uint16_t mask = (1U << len) - 1;
            return pop8[aligned & mask];
        };

        // ---------- 接缝评分（位并行） ----------
        auto seamScore = [&](int cx, bool isLeft) -> int {
            if (isLeft) {
                // 左缝：[cx - seamHalf, cx - 1] 黑，[cx, cx + seamHalf] 白
                int score = 0;

                // 左侧黑区（长度 = seamHalf）
                if (cx >= seamHalf) {
                    int white = popWhite(cx - seamHalf, seamHalf);
                    score += (seamHalf - white);
                } else if (cx > 0) {
                    int valid = cx;
                    int white = popWhite(0, valid);
                    score += (valid - white);
                }

                // 右侧白区（长度 = seamHalf + 1）
                int whiteLen = seamHalf + 1;
                if (cx < width) {  // 主动边界检查
                    int maxLen = width - cx;
                    if (whiteLen > maxLen) whiteLen = maxLen;
                    score += popWhite(cx, whiteLen);
                }
                
                return score;
            } else {
                // 右缝：[cx - seamHalf, cx] 白，[cx + 1, cx + seamHalf] 黑
                int score = 0;

                // 左侧白区（长度 = seamHalf + 1）
                int startL = (cx >= seamHalf) ? cx - seamHalf : 0;
                int lenL = cx - startL + 1;
                score += popWhite(startL, lenL);

                // 右侧黑区（长度 = seamHalf）
                if (cx + 1 < width) {
                    int remaining = width - (cx + 1);
                    int validR = seamHalf;
                    if (validR > remaining) validR = remaining;
                    if (validR > 0) {
                        int white = popWhite(cx + 1, validR);
                        score += validR - white;
                    }
                }

                return score;
            }
        };

        // ---------- 搜索左右边界 ----------
        int lmin = prevL - maxRadius;
        if (lmin < 0) {
            lmin = 0;
        }
        int lmax = prevL + maxRadius;
        if (lmax > width - 1) {
            lmax = width - 1;
        }
        int rmin = prevR - maxRadius;
        if (rmin < 0) {
            rmin = 0;
        }
        int rmax = prevR + maxRadius;
        if (rmax > width - 1) {
            rmax = width - 1;
        }

        int bestL = prevL, bestR = prevR;
        int bestLS = -1, bestRS = -1;

        for (int cx = lmin; cx <= lmax; ++cx) {
            int s = seamScore(cx, true);
            if (s > bestLS) { 
                bestLS = s; 
                bestL = cx; 
            }
        }
        for (int cx = rmin; cx <= rmax; ++cx) {
            int s = seamScore(cx, false);
            if (s > bestRS) { 
                bestRS = s;
                bestR = cx; 
            }
        }

        // 防交叉
        if (bestL >= bestR) {
            bestL = prevL;
            bestR = prevR;
        }


        leftCol[row] = bestL;
        rightCol[row] = bestR;
    }
}